#ifndef __SLAM_2D_ROS_H_
#define __SLAM_2D_ROS_H_

#include <ros/ros.h>
#include <tf2_ros/transform_broadcaster.h>
#include <geometry_msgs/TransformStamped.h>
#include <nav_msgs/OccupancyGrid.h>
#include <slam_2d_ros/common/slam_define.h>
#include <slam_2d_ros/mapping/mapping_2d.h>

#include <sstream>
using namespace std;

class Slam2D{
    public:
        Slam2D();
        ~Slam2D();

        // 雷达回调函数
        void ScanRawCallback(const sensor_msgs::LaserScan::Ptr& msg);
        // 里程计回调函数
        void OdomRawCallback(const nav_msgs::Odometry::Ptr& msg);
        // IMU回调函数
        void ImuRawCallback(const sensor_msgs::Imu::Ptr& msg);

        // 发布地图
        void PublishGlobalMap(ros::Publisher& map_pub, const cv::Mat& map_image, const Vec2f& origin);
        // 发布TF坐标
        void PublishBaseLinkTF(const Pose2D& current_pose);
        // 发布轨迹
        void PublishTrajectory(ros::Publisher& path_pub, const std::vector<Pose2D>& traj_poses);

    private:
        ros::NodeHandle nh_;
        ros::Subscriber scanRawSub_;
        ros::Subscriber odomSub_;
        ros::Subscriber imuSub_;
        ros::Publisher scanSlamPub_;
        ros::Publisher mapPub_;
        ros::Publisher trajPub_;

        slam_2d::Scan2DPtr scanRawPtr_;
        slam_2d::OdometryPtr odomRawPtr_;
        slam_2d::ImuPtr imuRawPtr_;
        slam_2d::Mapping2D mapping2D_;
};

#endif